/* +---------------------------------------------------------------------------+
   |                       Recursive World Toolkit                             |
   |                                                                           |
   |   Copyright (C) 2011  Jose Luis Blanco Claraco                            |
   |                                                                           |
   |      RWT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |    RWT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with  RWT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */

#include "rwt.h"

using namespace rwt;
using namespace std;

/** Save a MATLAB script with a representation of nodes,edges, etc. **/
void rwt::save_rwt_as_matlab_script(
	const RWT_World             & world,
	std::ostream                & o,
	const RWT_SaveMatlabOptions & opts
	)
{
	o <<
		"% Draw all waypoint nodes and their interconnections \n"
		"% Generated by RWT - MRPT " << mrpt::system::MRPT_getVersion() << " \n"
		"% ---------------------------------------------------\n\n";

	// edges:
	if (opts.edges_width>0)
	{
		o << "edges = [ ...\n";
		for (RWT_adjacency_graph::const_iterator it=world.graph.begin();it!=world.graph.end();++it)
		{
			const size_t idx1 = it->first.first;
			float x1,y1,z1;
			world.nodes.getPoint(idx1, x1,y1,z1);

			const size_t idx2 = it->first.second;
			float x2,y2,z2;
			world.nodes.getPoint(idx2, x2,y2,z2);

			o << x1 << " " << x2 << " "
			  << y1 << " " << y2 << " "
			  << z1 << " " << z2 << ";...\n";
		}
		o << "];\n"
		     "for i=1:size(edges,1)\n"
		     "  line( edges(i,1:2), edges(i,3:4), edges(i,5:6),'LineWidth'," << opts.edges_width <<",'Color',[0 0 1] );\n"
		     "end\n";
	}

	if (opts.nodes_size>0)
	{
		o << "hold on;\n";
		o << "nodes = [ ...\n";
		for (size_t i=0;i<world.nodes.size();i++)
		{
			float x,y,z;
			world.nodes.getPoint(i,x,y,z);
			o << x << " " << y << " "<< z << ";...\n";
		}
		o << "];\n"
			"plot3( nodes(:,1),nodes(:,2),nodes(:,3),'.','MarkerSize'," << opts.nodes_size <<",'Color',[1 0 0] );\n";
	}

	o << "axis equal;\n";
}
